/*
 * tarm_command.c
 *
 *  Created on: 2020年2月14日
 *      Author: AN
 */

#include "middleware/tarm_exit.h"
#include "hardware/tarm_communication.h"

#include "application/tarm_run.h"
#include "application/tarm_init.h"
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

/************************************************************
*	指令解析
*	传入参数：	指令
*	返回值：	无
*	执行位置：
*	注释：
*	命令格式：
*	bug说明：
************************************************************/
//等同于memccpy去掉c，同时还把dest最后一位清位\0，返回不包含c字符的长度
int memccpy_rid(char *dest, char *src, unsigned char c, unsigned int count)
{
    char *dest_s;

    dest_s = strchr(src, c);
    if(dest_s == NULL)
        return -1;

    memcpy(dest, src, strlen(src) - strlen(dest_s));
    dest[strlen(src) - strlen(dest_s)] = '\0';

    return strlen(src) - strlen(dest_s);
}

//等同于strstr去掉str2
char* strstr_rid(char * str1, char * str2 )
{
    char* locate = str1;
    locate = strchr(str1,'&');
    if(locate==NULL)
        return NULL;
    return ++locate;

//    if(locate==NULL)
//        return NULL;

//    locate += strlen(str2);
//    return locate;
}

double atof_my(const char *str)
{
    double s=0.0;

    double d=10.0;
    int jishu=0;

    uint8_t falg=0;

    while(*str==' ')
    {
        str++;
    }

    if(*str=='-')//记录数字正负
    {
        falg=1;
        str++;
    }

    if(!(*str>='0'&&*str<='9'))//假设一開始非数字则退出。返回0.0
        return s;

    while(*str>='0'&&*str<='9'&&*str!='.')//计算小数点前整数部分
    {
        s=s*10.0+*str-'0';
        str++;
    }

    if(*str=='.')//以后为小树部分
        str++;

    while(*str>='0'&&*str<='9')//计算小数部分
    {
        s=s+(*str-'0')/d;
        d*=10.0;
        str++;
    }

    if(*str=='e'||*str=='E')//考虑科学计数法
    {
        str++;
        if(*str=='+')
        {
            str++;
            while(*str>='0'&&*str<='9')
            {
                jishu=jishu*10+*str-'0';
                str++;
            }
            while(jishu>0)
            {
                s*=10;
                jishu--;
            }
        }
        if(*str=='-')
        {
            str++;
            while(*str>='0'&&*str<='9')
            {
                jishu=jishu*10+*str-'0';
                str++;
            }
            while(jishu>0)
            {
                s/=10;
                jishu--;
            }
        }
    }

    return s*(falg?-1.0:1.0);
}

int search4(char *originate, uint16_t size, char *command, double* command_num)    //定长指令
{
    int CommandNum = 3;
//    char command[3]="0";
//    double command_num[3]={0};
    char SearchChar[_UARTSAVEBUFSIZE];
    char assis_str[30]="\0";
//    char ReportStr[100]="\0";
	int i;
    memcpy(SearchChar, originate, size);
    char *one = strstr_rid(SearchChar, "_&");
//*
    if(one == NULL)
        return -1;

    for(i=1;i<=CommandNum;i++)
    {
    	//获取命令
    	command[i-1] = one[0];
    	one += 1;

    	//获取参数字符串
    	memccpy_rid(assis_str, one, ';', strlen(one));
//        PRINTF_UART1("start8\n");
    	if(assis_str == NULL)
    		return i-1;
//        PRINTF_UART1("start9\n");


    	//转成参数
        PRINTF_UART1(assis_str);
//        command_num[i-1] = atof(assis_str);
        int t_d = atof_my(assis_str);
//        PRINTF_UART1("start6\n");
        command_num[i-1] = (double)t_d;
//        PRINTF_UART1("start7\n");

    	//移位
    	one += (strlen(assis_str)+1);
//        PRINTF_UART1("start5\n");

//    	if(strchr(one, ';') == NULL)
//    		{return i;PRINTF_UART1("start3\n");}
//        int len = strlen(one);
//        sprintf(ReportStr,"\r\n%d\r\n", len);
//        PRINTF_UART1(ReportStr);
//        PRINTF_UART1("start2\n");
//        for(int j=0; j<len; j++)
//        {
//            if(one[j]==';') break;
//            else if(j==len-1) {return i;PRINTF_UART1("start3\n");}
//        }
        if(*one<='A'||*one>='Z') {return i;}

//        PRINTF_UART1("start4\n");
    }
//*/
    return 3;
}

//command_operate
//USART_RX_BUF: the rx buffer
//USART_RX_STA: how many comment in the rx buffer
//return :1(success);	-1(error)
int command_operate(uint8_t *USART_RX_BUF,uint16_t RX_BUF_SIZE)
{
	double x_dis=g_TARM_RunRecord.pv_current.x,y_dis=g_TARM_RunRecord.pv_current.y;
	uint16_t speed;

    char SearchChar[_UARTSAVEBUFSIZE];
	char command[3]="0";
    double command_num[3]={0};

	uint16_t len = RX_BUF_SIZE;
	uint8_t CommandCount = 0;

	//1-解码
	if(RX_BUF_SIZE<5) return -1;
	memcpy(SearchChar, USART_RX_BUF, len);
	CommandCount = search4(SearchChar, len, command, command_num);
//*
	const char t_command_judge1 = command[0];
	const char t_command_judge2 = command[1];
	const char t_command_judge3 = command[2];
	const double t_command_num_record1 = command_num[0];
	const double t_command_num_record2 = command_num[1];
	const double t_command_num_record3 = command_num[2];


	if(CommandCount <= 0){
		ReportStruct.Uart1ReportFlag.VerifyError = 1;
		ReportStruct.Uart2ReportFlag.VerifyError = 1;
		return -1;
	}

	//2.2-解析
	switch(t_command_judge1)
	{
		case 'F':
			x_dis = g_TARM_RunRecord.pv_current.x+t_command_num_record1;
			break;
		case 'B':
			x_dis = g_TARM_RunRecord.pv_current.x-t_command_num_record1;
			break;
		case 'U':
			y_dis = g_TARM_RunRecord.pv_current.y+t_command_num_record1;
			break;
		case 'D':
			y_dis = g_TARM_RunRecord.pv_current.y-t_command_num_record1;
			break;
		case 'R':
			g_TARM_RunRecord.r_val.d = t_command_num_record1;
			break;
		case 'X':
			x_dis = t_command_num_record1;
			if(t_command_judge2 == 'Y')
				y_dis = t_command_num_record2;
			break;
		case 'C':
			g_TARM_RunRecord.ClickNum = t_command_num_record1;
			break;
		case 'T':
			g_TARM_RunRecord.TurnNum = t_command_num_record1;
			break;
		case 'S':
			break;
		case 'E':
			break;
        case 'J':
            break;
		default:
			ReportStruct.Uart1ReportFlag.VerifyError = 1;
			ReportStruct.Uart2ReportFlag.VerifyError = 1;
			return -2;
	}

	if(CommandCount == 2 && t_command_judge2 == 'S')
	{
		speed = (uint16_t)t_command_num_record2;
	}
	else if(CommandCount == 3 && t_command_judge1 == 'X'  && t_command_judge2 == 'Y' && t_command_judge3 == 'S')
	{
		speed = (uint16_t)t_command_num_record3;
	}
	else
		speed = 250;

//	char reportBuf_tarm_communication_command_operate[50];
//	sprintf(reportBuf_tarm_communication_command_operate,
//			"\r\nspeed:%d\r\n",speed);
//	PRINTF_UART0(reportBuf_tarm_communication_command_operate);
//	PRINTF_UART1(reportBuf_tarm_communication_command_operate);

	//3-执行
	if(t_command_judge1 == 'R'){
		point_to_point_rotation(g_TARM_RunRecord.r_val.d,speed);
		ReportStruct.Uart1ReportFlag.VerifySuccess = 1;
		ReportStruct.Uart2ReportFlag.VerifySuccess = 1;
	}
	else if(t_command_judge1 == 'C'){
		click_pwm();
		ReportStruct.Uart1ReportFlag.ExecuteFinish = 1;
		ReportStruct.Uart2ReportFlag.ExecuteFinish = 1;
	}
	else if(t_command_judge1 == 'T'){
		turn_pwm();
		ReportStruct.Uart1ReportFlag.ExecuteFinish = 1;
		ReportStruct.Uart2ReportFlag.ExecuteFinish = 1;
	}
	else if(t_command_judge1 == 'S'){
		Limit_Disable();
		io_init();
		locate_init();
		Limit_Enable();
		ReportStruct.Uart1ReportFlag.ExecuteFinish = 1;
		ReportStruct.Uart2ReportFlag.ExecuteFinish = 1;
	}
	else if(t_command_judge1 == 'E'){
		if(!(g_TARM_RunRecord.RunFlag_p ||
				g_TARM_RunRecord.RunFlag_v ||
				g_TARM_RunRecord.RunFlag_r))
		{
			ReportStruct.Uart1ReportFlag.ExecuteFinish = 1;
			ReportStruct.Uart2ReportFlag.ExecuteFinish = 1;
		}
		else
		{
			ReportStruct.Uart1ReportFlag.VerifySuccess = 1;
			ReportStruct.Uart2ReportFlag.VerifySuccess = 1;
		}
	}
	else if(t_command_judge1 == 'J'){
	    CloseLoopUpdate();
        ReportStruct.Uart1ReportFlag.ExecuteFinish = 1;
        ReportStruct.Uart2ReportFlag.ExecuteFinish = 1;
	}
	else{
		if(point_to_point_xy_pvthr(x_dis,y_dis,speed)==-1)
		{
			ReportStruct.Uart1ReportFlag.VerifyError = 1;
			ReportStruct.Uart2ReportFlag.VerifyError = 1;
		}
		ReportStruct.Uart1ReportFlag.VerifySuccess = 1;
		ReportStruct.Uart2ReportFlag.VerifySuccess = 1;
	}
//*/
	return 1;
}
